#include "Matrix.h"
#include <cmath>

Matrix44::Matrix44()
{	
}

Matrix44::Matrix44(const Matrix44 &matrix)
{
	for(int i = 0; i < 16; i++)
		field[i] = matrix.field[i];
}
Matrix44 Matrix44::I()
{
	Matrix44 m;
	for(int i = 0; i < 16; i++)
		m.field[i] = 0.0f;
	m.field[0] = 1.0f;
	m.field[5] = 1.0f;
	m.field[10] = 1.0f;
	m.field[15] = 1.0f;
	return m;
}

Matrix44 Matrix44::RotateX(float angle)
{
	Matrix44 m(Matrix44::I());
	m.field[5] = cos(angle);
	m.field[6] = -sin(angle);
	m.field[9] = sin(angle);
	m.field[10] = cos(angle);
	return m;
}
Matrix44 Matrix44::RotateY(float angle)
{
	Matrix44 m(Matrix44::I());
	m.field[0] = cos(angle);
	m.field[2] = sin(angle);
	m.field[8] = -sin(angle);
	m.field[10] = cos(angle);
	return m;
}
Matrix44 Matrix44::RotateZ(float angle)
{
	Matrix44 m(Matrix44::I());
	m.field[0] = cos(angle);
	m.field[1] = -sin(angle);
	m.field[4] = sin(angle);
	m.field[5] = cos(angle);
	return m;
}
Matrix44 Matrix44::Translate(Vector3 &v)
{
	Matrix44 m(Matrix44::I());
	m.field[3] = v.x;
	m.field[7] = v.y;
	m.field[11] = v.z;	
	return m;
}
Matrix44 Matrix44::Scale(Vector3 &v)
{
	Matrix44 m(Matrix44::I());
	m.field[0] = v.x;
	m.field[5] = v.y;
	m.field[10] = v.z;	
	return m;
}

Matrix44& Matrix44::operator = (Matrix44 &matrix)
{
	for(int i = 0;i < 16; i++)
		field[i] = matrix.field[i];
	return *this;
}
Matrix44 operator * (Matrix44 &m1, Matrix44 &m2)
{
	Matrix44 result;
	int x, y, w;
	for(x = 0;x < 4; x++)
	{
		for(y = 0;y < 4; y++)
		{
			result.field[4 * y + x]=0.0f;
			for(w = 0;w < 4; w++)
				result.field[4 * y + x] += m1.field[w + 4 * y] * m2.field[x + 4 * w];
		}
	}
	return result;
}

Vector3 operator * (Matrix44 &m,Vector3 &v)
{
	Vector3 result;
	result.x = v.x * m.field[0] + v.y * m.field[1] + v.z * m.field[2] + m.field[3];
	result.y = v.x * m.field[4] + v.y * m.field[5] + v.z * m.field[6] + m.field[7];
	result.z = v.x * m.field[8] + v.y * m.field[9] + v.z * m.field[10] + m.field[11];
	float w = 1.0f / (v.x * m.field[12] + v.y * m.field[13] + v.z * m.field[14] + m.field[15]);
	result.x *= w;
	result.y *= w;
	result.z *= w;
	return result;
}
Matrix44& Matrix44::Transpose(void)
{
	float a;
	for(int i = 0; i < 4; i++)
	{
		for(int j = i; j < 4; j++)
		{
			a = field[4 * i + j];
			field[4 * i + j] = field[4 * j + i];
			field[4 * j + i] = a;
		}
	}
	return *this;
}